/**
 * @file Plate.cpp
 * @author Anthracene
 * @brief 
 * @version 0.1
 * @date 2018-10-16
 * 
 * @copyright Copyright (c) 2018
 * 
 */
#include "Plate.h"
//#include "Serial_Port.h"
#include "hsv_Detect.h"
#include <iostream>
#include <string.h>
#include <opencv/cv.hpp>
#include <cmath>

int id_base = 0;

/**
 * @brief Construct a new Light Bar:: Light Bar object (Blank)
 * 
 */
LightBar::LightBar()
{
}

/**
 * @brief Construct a new Light Bar:: Light Bar object
 * 
 * @param _Top 
 * @param _Bottom 
 */
LightBar::LightBar(cv::Point2f _Top, cv::Point2f _Bottom)
{
    _init_(_Top, _Bottom);
}

void LightBar::_init_(cv::Point2f _Top, cv::Point2f _Bottom)
{
    Top = _Top;
    Bottom = _Bottom;
    length = sqrt(pow(Top.x-Bottom.x, 2)+pow(Top.y-Bottom.y, 2));
    Center.x = (Top.x+Bottom.x)/2;
    Center.y = (Top.y+Bottom.y)/2;
    id = id_base;
    id_base++;
    empty = false;
}

double LightBar::ranging(LightBar _Bar)
{
    double res = sqrt(pow(Center.x-_Bar.Center.x, 2)+pow(Center.y-_Bar.Center.y, 2));
    return res;
}

bool LightBar::judge(LightBar _Bar)
{
    //判断依据1
    if(ranging(_Bar)/length < 2.9)
        return true;
}

/**
 * @brief Construct a new Armor Plate:: Armor Plate object
 * 
 */
ArmorPlate::ArmorPlate()
{
}

/**
 * @brief Construct a new Armor Plate:: Armor Plate object
 * 
 * @param _Bar_I 
 * @param _Bar_II 
 */
ArmorPlate::ArmorPlate(LightBar _Bar_I, LightBar _Bar_II)
{
    _init_(_Bar_I, _Bar_II);
}

void ArmorPlate::_init_(LightBar _Bar_I, LightBar _Bar_II)
{
    if(_Bar_I.Center.x < _Bar_II.Center.x)
    {
        left_Bar = _Bar_I;
        right_Bar = _Bar_II;
    }
    else
    {
        left_Bar = _Bar_II;
        right_Bar = _Bar_I;
    }
    //需修改 获取中点的方式
    Center.x = (left_Bar.Center.x+right_Bar.Center.x)/2;
    Center.y = (left_Bar.Center.y+right_Bar.Center.y)/2;
    corner[0] = left_Bar.Top;
    corner[1] = right_Bar.Top;
    corner[2] = left_Bar.Bottom;
    corner[3] = right_Bar.Bottom;
    empty = false;
}

void ArmorPlate::print()
{
    //printf("-------Current Plate-------\n");
    //printf("LT:(%d,%d)    RT:(%d,%d)\n\n",int(corner[0].x),int(corner[0].y),int(corner[1].x),int(corner[1].y));
    printf("    Center:(%d,%d)\r",int(Center.x),int(Center.y));
    //printf("LB:(%d,%d)    RB:(%d,%d)\n\n",int(corner[2].x),int(corner[2].y),int(corner[3].x),int(corner[3].y));
}

/*
void ArmorPlate::sendLoca()
{
    std::stringstream data;
    data<<int(Center.x)<<int(Center.y)<<"\n";
    std::string dataBuff = data.str();
    const char *sendBuff = dataBuff.c_str();
    sendData(&port1, sendBuff);
    //std::cout<<data.str()<<std::endl;
}
*/

/**
 * @brief Get Frames from Video
 * 
 * @param fileName 
 * @param fps 
 */
void get_Frame_Video(std::string fileName, int fps)
{
    cv::VideoCapture capture(fileName);
    cv::VideoWriter writer;
    const std::string savepath = "../video/saved.avi";
    int codec = CV_FOURCC('M','J','P','G');
    cv::Size video_size;
    if(capture.isOpened() == false)
        perror("can not open the video");
    bool in_flag = false;
    while(true)
    {
        capture >> frame_Buff;
        if(in_flag == false)
        {
            video_size = frame_Buff.size();
            writer.open(savepath, codec, fps, video_size);
            in_flag = true;
        }
        if(frame_Buff.empty() == true)
        {
            perror("empty frame");
            cv::waitKey(1000);
            return;
        }
        detect_HScolor(frame_Buff, frame_HSV);
        #ifdef _DEBUG
        cv::imshow("video", frame_Buff);
        #endif
        locate_Plate(frame_HSV);
        writer << frame_Buff;
        cv::waitKey(int(1000/fps));
    }
}

/**
 * @brief Locate the plate
 * 
 * @param hsv_img 
 */
void locate_Plate(cv::Mat hsv_img)
{
    cv::Mat bin_img;
    cv::cvtColor(hsv_img, bin_img, cv::COLOR_BGR2GRAY);
    cv::GaussianBlur(bin_img, bin_img, cv::Size(3,3), 1);
    int thersh = 80;//二值化阈值
    cv::threshold(bin_img, bin_img, thersh, 255, cv::THRESH_BINARY);

    cv::Mat element = cv::getStructuringElement(
		cv::MORPH_ELLIPSE, cv::Size(5, 5)
	);//腐蚀膨胀掩膜
    for (int i = 0; i < 4; i++)
		cv::dilate(bin_img, bin_img, element);//腐蚀
	for (int i = 0; i < 4; i++)
		cv::erode(bin_img, bin_img, element);//膨胀
    
    //寻找轮廓
    std::vector<std::vector<cv::Point> > contours;
	std::vector<cv::Vec4i> hierarcy;
	cv::findContours(bin_img, contours, hierarcy, 0, CV_CHAIN_APPROX_NONE);
	std::vector<cv::Rect> boundRect(contours.size());
	std::vector<std::vector<cv::Point> > conPoints(contours.size());

    std::vector<cv::RotatedRect> minRect(contours.size());

    //最多不超过10块装甲板
    std::vector<LightBar> lightBar(20);
    std::vector<ArmorPlate> armorPlate(10);
    int LightBar_count = 0;
    int ArmorPlate_count = 0;

    for (int i = 0; i < contours.size(); i++)
	{
		//最小外接矩形近似
		cv::approxPolyDP(cv::Mat(contours[i]), conPoints[i], 15, true);
		boundRect[i] = cv::boundingRect(cv::Mat(conPoints[i]));
		double k = boundRect[i].height / double(boundRect[i].width);
		double s = boundRect[i].height * double(boundRect[i].width);
		//筛选出合适大小的矩形
		if (k > 2.5 && s > 60 && LightBar_count < 20)
		{
			//cv::rectangle(frame_Buff, boundRect[i].tl(), boundRect[i].br(),
			//	cv::Scalar(100, 255, 100), 4, 8, 0);
			minRect[i] = cv::minAreaRect(contours[i]);
            cv::Point2f rect_Points[4];
            minRect[i].points(rect_Points);
            //处理每个旋转矩形void sendLoca();的四个点
            //排序 先按y值降序 void sendLoca();再按x值分别降序 排序结果：左上->右下
            int count_times = 1;
            float y_max = 0, x_max = 0;
            while(count_times != 0)
            {
                count_times = 0;
                for(int i = 0;i < 3;i++)
                {
                    if(rect_Points[i].y > rect_Points[i+1].y)
                    {
                        std::swap(rect_Points[i], rect_Points[i+1]);
                        count_times++;//计数，如果没有需要调换次序则退出循环
                    }
                }
            }
            if(rect_Points[0].x > rect_Points[1].x)
                std::swap(rect_Points[0], rect_Points[1]);
            if(rect_Points[2].x > rect_Points[3].x)
                std::swap(rect_Points[2], rect_Points[3]);
            //找出灯条的上下顶点
            cv::Point2f Top_LightBar, Bottom_LightBar;
            Top_LightBar.x = (rect_Points[0].x+rect_Points[1].x)/2;
            Top_LightBar.y = (rect_Points[0].y+rect_Points[1].y)/2;
            Bottom_LightBar.x = (rect_Points[2].x+rect_Points[3].x)/2;
            Bottom_LightBar.y = (rect_Points[2].y+rect_Points[3].y)/2;
            //新建一个灯条
            lightBar[LightBar_count]._init_(Top_LightBar, Bottom_LightBar);
            //画点(在DEBUG模式下)
            //#ifdef _DEBUG
            cv::circle(frame_Buff, Top_LightBar, 4, cv::Scalar(255,100,100), -1);
            cv::circle(frame_Buff, Bottom_LightBar, 4, cv::Scalar(255,100,100), -1);
            cv::circle(frame_Buff, lightBar[LightBar_count].Center, 4, cv::Scalar(100,255,100), -1);
            //#endif
            LightBar_count++;
		}
	}
    for(int i = 0;i < LightBar_count;i++)
        for(int j = 0;j < LightBar_count;j++)
        {
            //如果被确定为装甲板 则新建一个装甲板类
            if(lightBar[i].judge(lightBar[j]) && i!=j)
            {
                armorPlate[ArmorPlate_count]._init_(lightBar[i], lightBar[j]);
                armorPlate[ArmorPlate_count].print();
                //#ifdef _DEBUG
                //armorPlate[ArmorPlate_count].print();
                cv::line(frame_Buff, armorPlate[ArmorPlate_count].corner[0], armorPlate[ArmorPlate_count].corner[3], cv::Scalar(100,100,255), 2);
                cv::line(frame_Buff, armorPlate[ArmorPlate_count].corner[1], armorPlate[ArmorPlate_count].corner[2], cv::Scalar(100,100,255), 2);
                cv::circle(frame_Buff, armorPlate[ArmorPlate_count].Center, 4, cv::Scalar(255,100,100), -1);
                //#endif
                ArmorPlate_count++;
            }
        }
    #ifdef _DEBUG
    cv::imshow("bin", frame_Buff);
    #endif
}
